﻿/*
 * Copyright © 2020, Wuxi Safover Intelligent Technology Co., Ltd. All rights reserved.
 * 
 * 无锡安永智能科技有限公司版权所有，未经许可，不得用于商业用途
 * 
 * Visit: https://www.safover.com/ 
 */

using log4net;
using System;
using System.Reflection;
using Safover.LidarCollector.Models;

namespace Safover.LidarCollector.Managers
{
    public class HorizontalScanControlLogic
    {
        private static readonly ILog Log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);

        private static string Name => "HorizontalScanControlLogic";

        public int CircularConstant { get; } = 360;

        public float StartAngle { get; }

        /// <summary>
        /// 需扫描的角行程量
        /// </summary>
        public float RangeAngle { get; }


        public float EndAngle => (StartAngle + RangeAngle) % CircularConstant;

        public float StepAngle { get; }

        public float CurrentAngle { get; private set; }


        public float FixedVerticalAngle { get; private set; }

        HorizontalBlindSectorConfig[] BlindSectors { get; set; }

        public bool AbleToScan
        {
            get
            {
                var scanedAngleRange = CurrentAngle - StartAngle;
                return scanedAngleRange < (RangeAngle + StepAngle);
            }
        }

        public HorizontalScanControlLogic(float startAngle, float rangeAngle, float stepAngle, float fixedVerticalAngle, HorizontalBlindSectorConfig[] blindSectors = null)
        {
            CheckInput(startAngle, rangeAngle, stepAngle);

            StartAngle = startAngle;
            RangeAngle = rangeAngle;
            StepAngle = stepAngle;
            FixedVerticalAngle = fixedVerticalAngle;

            CurrentAngle = StartAngle;

            BlindSectors = blindSectors;
        }

        private void CheckInput(float startAngle, float rangeAngle, float stepAngle) 
        {
            // 起始角度：0-359
            if (startAngle < 0 || startAngle >= CircularConstant)
            {
                throw new ArgumentOutOfRangeException($"{Name}: startAngle=[{startAngle}] is out of the range=[0 - {CircularConstant})");
            }

            // 行程角度量：0-359
            if (rangeAngle <= 0 || rangeAngle > CircularConstant)
            {
                throw new ArgumentOutOfRangeException($"{Name}: rangeAngle=[{rangeAngle}] is out of the range=(0 - {CircularConstant}]");
            }

            // 步长角度必须大于0
            if (stepAngle <= 0)
            {
                throw new ArgumentOutOfRangeException($"{Name}: stepAngle=[{stepAngle}] should be within [0-90)");
            }
        }

        public HScanInstruction Tick()
        {
            var instruction = DoTick();

            Log.DebugWriteLine($"{Name}: current instruction=[{instruction}].");

            return instruction;
        }

        private HScanInstruction DoTick()
        {
            if (!AbleToScan)
            {
                return new HScanInstruction(false, null, FixedVerticalAngle);
            }

            var instruction = new HScanInstruction(true, CurrentAngle % CircularConstant, FixedVerticalAngle);

            CurrentAngle += StepAngle;

            return WithinBlind(instruction) ? DoTick() : instruction;
        }

        

        private bool WithinBlind(HScanInstruction instruction)
        {
            var blindSectors = BlindSectors;

            if (blindSectors == null || blindSectors.Length <= 0)
            {
                return false;
            }

            foreach (HorizontalBlindSectorConfig blindSector in blindSectors)
            {
                if (blindSector.IsInside((float)instruction.ScanAngle))
                {
                    Log.DebugWriteLine($"{Name} scan angle=[{instruction.ScanAngle}] IS within blind sector=[{blindSector}].");
                    return true;
                }

                Log.DebugWriteLine($"{Name} scan angle=[{instruction.ScanAngle}] IS NOT within blind sector=[{blindSector}].");
            }

            return false;
        }

        public override string ToString()
        {
            return $"StartAngle={StartAngle}, RangeAngle={RangeAngle}, StepAngle={StepAngle}, CurrentAngle={CurrentAngle}, AbleToScan={AbleToScan}.";
        }
    }

}
